path(path,'../libs/publicMatlabLibs/');
cl
datafileFolder = '../data/simResults/pesudosim/';

initialTime = 508*604800+388804.50;

R  = [1e7,3e7,5e7];
w  = 5e-3*[0.99,0.20123,0.032413];
centers =[0,0,0;0,0,0;0,0,0];


for satnum =  1:1
    load(strcat(datafileFolder,'sat',num2str(satnum),'.mat'));
    kineStateCovslength = size(kineStateCovs,3);
    
    startprint = 2;
    endprint =  kineStateCovslength;
%     endprint =100;
    timeTagsRelative  = (timeTags'-timeTags(1))/3600;
    trs       = traceSeries(kineStateCovs(1:2,1:2,startprint:endprint));
    figure(1);
    plot(timeTagsRelative(startprint:endprint),sqrt(trs),'r+'); 
    hold on 
    plot(timeTagsRelative(startprint:endprint),sqrt(trs));
    figure(2)
    plot(timeTagsRelative,kineStateMeans(1,:))
    hold on 
       
%     startprint = 1;
    deltatime = timeTags - initialTime;
    tmpIndex = length(deltatime);
    posexpected = satpos(w(satnum),deltatime,R(satnum),centers(:,1));
    pickedKinePoints = kineStateMeans;
    kineTrueState =  posexpected;
    rmse = (pickedKinePoints(1:1,startprint:tmpIndex) -kineTrueState(1:1,startprint:tmpIndex));
    rmsez = (pickedKinePoints(3,startprint:tmpIndex) -kineTrueState(3,startprint:tmpIndex));
    figure(3);
    plot(timeTagsRelative(startprint:tmpIndex),rmse);
    figure(4);
    plot(timeTagsRelative(startprint:tmpIndex),abs(rmsez)');
    figure(5);
    plot(timeTagsRelative(startprint:tmpIndex),sqrt(sum(rmse.^2,1)));
%     figure();
%     plot(timeTrueTagsRelative(1:tmpIndex),rmsev);
end


